//
// Created by yu on 24-4-24.
//
#include "LQRControl.h"

////通过两电机角度计算腿长，两腿高平均数
double LQR::LegLengthCalculate()
{
    double X1 = 42.23;
    double X2 = 88.43;
    double X3 = 19.99;
    double X4 = 108.69;
    double X5 = 122.62;
    double XX1 = pow(X1, 2);
    double XX2 = pow(X2, 2);
    double XX3 = pow(X3, 2);
    double XX4 = pow(X4, 2);
    double XX5 = pow(X5, 2);
    double staticAngle = 19.045 * M_PI / 180;

    double BD = pow((XX1 + XX4 - 2 * X1 * X4 * cos(staticAngle + leftAngle)), 0.5);
    double angleABD = acos((BD * BD + XX4 - XX1) / (2 * X4 * BD));
    double angleDBC = acos((BD * BD + XX3 - XX2) / (2 * BD * X3));
    double angleY = M_PI - leftAngle - angleABD - angleDBC;
    double S = pow((XX4 + XX5 - 2 * X4 * X5 * cos(leftAngle + angleY)), 0.5);
    double T = acos((S * S + XX4 - XX5) / (2 * S * XX4));
    double theta = M_PI - leftAngle - T;
    double H = sin(theta) * S;
    LQR::leftLength = sin(staticAngle) * X1 / 2 + H - 49.46;

    double BD1 = pow((XX1 + XX4 - 2 * X1 * X4 * cos(staticAngle + rightAngle)), 0.5);
    double angleABD1 = acos((BD1 * BD1 + XX4 - XX1) / (2 * X4 * BD1));
    double angleDBC1 = acos((BD1 * BD1 + XX3 - XX2) / (2 * BD1 * X3));
    double angleY1 = M_PI - rightAngle - angleABD1 - angleDBC1;
    double S1 = pow((XX4 + XX5 - 2 * X4 * X5 * cos(rightAngle + angleY1)), 0.5);
    double T1 = acos((S1 * S1 + XX4 - XX5) / (2 * S1 * XX4));
    double theta1 = M_PI - rightAngle - T1;
    double H1 = sin(theta1) * S1;
    LQR::rightLength = sin(staticAngle) * X1 / 2 + H1 - 49.46;
}


std::vector<double> LQR::KMatrixCalculate(double legLength)
{
    LQR::currentKMatrix = {-1.0000, -4.0006, 69.1075, 7.0375};
}

void LQR::LQRBalanceControl()
{

}